#include "senova.h"

VehicleCtrlOps mSenovaCtrlOps;

CanTxMsg control_temp =
{
    0x725, 0x18db33f1,
    CAN_ID_STD, CAN_RTR_DATA,
    8,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

CanTxMsg function_test =
{
    0x725, 0x18db33f1,
    CAN_ID_STD, CAN_RTR_DATA,
    8,
    0x02, 0x10, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00
};

void senova_init(Vehicle *vehicle)
{
    logi("%s", __func__);
    mSenovaCtrlOps.support |= SUPPORT_CONTROL_WINDOW;
    mSenovaCtrlOps.support |= SUPPORT_CONTROL_DOOR;
    mSenovaCtrlOps.support |= SUPPORT_CONTROL_LIGHT;
    //    mSenovaCtrlOps.support |= SUPPORT_CONTROL_SUNROOF;
    mSenovaCtrlOps.support |= SUPPORT_CONTROL_TRUNK;
    mSenovaCtrlOps.support |= SUPPORT_CONTROL_FINDCAR;

    mSenovaCtrlOps.control_window = senova_control_window;
    mSenovaCtrlOps.control_door = senova_control_door;
    mSenovaCtrlOps.control_light = senova_control_light;
    mSenovaCtrlOps.control_sunroof = senova_control_sunroof;
    mSenovaCtrlOps.control_trunk = senova_control_trunk;
    mSenovaCtrlOps.control_findcar = senova_control_findcar;


    vehicle->ctrlOps = &mSenovaCtrlOps;
}

unsigned long senova_get_auth(unsigned long seed)
{
    unsigned long TempSeed,TempSeeds;
    int i;
    unsigned long TempSeed_bit[32]=
    {
        0x71725adc,0xe2e4b5b8,0x9deec8c6,0x63fa323a,
        0xc7f46474,0xd7cf6b5e,0xf7b9750a,0xb75549a2,
        0x368d30f2,0x6d1a61e4,0xda34c3c8,0xec4e2426,
        0x80bbebfa,0x59507442,0xb2a0e884,0x3d6672be,
        0x7acce57c,0xf599caf8,0xb3143646,0x3e0fcf3a,
        0x7c1f9e74,0xf83f3ce8,0xa859da66,0x0894177a,
        0x11282ef4,0x22505de8,0x44a0bbd0,0x894177a0,
        0x4aa54cf6,0x954a99ec,0x72b2906e,0xe56520dc
    };
    TempSeeds = seed;
    TempSeed = 0;

    for(i=0;i<32;i++)
        TempSeed ^= (((TempSeeds>>i)&1)*TempSeed_bit[i]);

    return (TempSeed);
}

void senova_auth(void)
{
    CanTxMsg txMsg = {
        0x725, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
    };
    CanRxMsg rxMsg;

    uint8_t i;
    uint8_t init[8] = {0x02, 0x10, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00};
    uint8_t request[8] = {0x02, 0x27, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00};
    uint8_t auth[8] = {0x06, 0x27, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00};
    uint8_t seed[4];
    uint8_t key[4];
    unsigned long longSeed;
    unsigned long longKey;

    logi("%s", __func__);
    bsp_can_clear();
    bsp_can_filter(0x7a5, 0x7a5, 0x7ff, 0x7ff);

    for(i = 0; i < 8; i++) {
        txMsg.Data[i] = init[i];
    }

    if(vehicle_can_session(&txMsg, &rxMsg)) {
        logi("%s: return [%02x %02x %02x]", __func__, rxMsg.Data[0],
                rxMsg.Data[1], rxMsg.Data[2]);
    } else {
        loge("%s: failed to send init data", __func__);
        return;
    }

    for(i = 0; i < 8; i++) {
        txMsg.Data[i] = request[i];
    }

    if(vehicle_can_session(&txMsg, &rxMsg)) {
        logi("%s: return [%02x %02x %02x]", __func__, rxMsg.Data[0],
                rxMsg.Data[1], rxMsg.Data[2]);
        //save seed
        for(i = 0; i < 4; i++) {
            seed[i] = rxMsg.Data[i + 3];
        }
        logi("%s: seed [%02x %02x %02x %02x]", __func__, seed[0],
                seed[1], seed[2], seed[3]);
        longSeed = (seed[0] << 24 |
                    seed[1] << 16 |
                    seed[2] << 8  |
                    seed[3]);
        logi("%s: seed = %x", __func__, longSeed);
        longKey = senova_get_auth(longSeed);
        logi("%s: key = %x", __func__, longKey);
        key[0] = (longKey >> 24) & 0xff;
        key[1] = (longKey >> 16) & 0xff;
        key[2] = (longKey >> 8)  & 0xff;
        key[3] = longKey & 0xff;
    } else {
        loge("%s: failed to send request data", __func__);
        return;
    }
    for(i = 0; i < 4; i++) {
        auth[i + 3] = key[i];
    }

    for(i = 0; i < 8; i++) {
        txMsg.Data[i] = auth[i];
    }

    if(vehicle_can_session(&txMsg, &rxMsg)) {
        logi("%s: auth [%02x %02x %02x]", __func__, rxMsg.Data[0],
                rxMsg.Data[1], rxMsg.Data[2]);
    } else {
        loge("%s: failed to send auth data", __func__);
        return;
    }
}

void senova_control_window(uint8_t state)
{
    uint8_t i;
    uint8_t cmds_on[4][8] = 
    {
        { 0x10, 0x0C, 0x2F, 0x58, 0xA3, 0x03, 0x80, 0x00 },
        { 0x21, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00 },
        { 0x10, 0x0C, 0x2F, 0x58, 0xA3, 0x03, 0x00, 0x00 },
        { 0x21, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00 },
    };

    uint8_t cmds_off[4][8] = 
    {
        { 0x10, 0x0C, 0x2F, 0x58, 0xA3, 0x03, 0x40, 0x00 },
        { 0x21, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00 },
        { 0x10, 0x0C, 0x2F, 0x58, 0xA3, 0x03, 0x00, 0x00 },
        { 0x21, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00 },
    };

    logi("%s: state = %d", __func__, state);
    senova_auth();
    os_delay_ms(200);
//    bsp_can_send(&function_test);
    if(state) {
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_on[0][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_on[1][i];
        }
        bsp_can_send(&control_temp);
        os_delay(5);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_on[2][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_on[3][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
    } else {
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_off[0][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_off[1][i];
        }
        bsp_can_send(&control_temp);
        os_delay(5);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_off[2][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_off[3][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
    }
    bsp_can_clear();
}

void senova_control_door(uint8_t state)
{
    uint8_t i;
    uint8_t cmds_on[4][8] = 
    {
        { 0x10, 0x0C, 0x2F, 0x58, 0xA5, 0x03, 0x08, 0x00 },
        { 0x21, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00 },
        { 0x10, 0x0C, 0x2F, 0x58, 0xA5, 0x03, 0x00, 0x00 },
        { 0x21, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00 },
    };

    uint8_t cmds_off[4][8] = 
    {
        { 0x10, 0x0C, 0x2F, 0x58, 0xA5, 0x03, 0x04, 0x00 },
        { 0x21, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00 },
        { 0x10, 0x0C, 0x2F, 0x58, 0xA5, 0x03, 0x00, 0x00 },
        { 0x21, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00 },
    };

    logi("%s: state = %d", __func__, state);
    senova_auth();
    os_delay_ms(200);
//    bsp_can_send(&function_test);
    if(state) {
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_on[0][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_on[1][i];
        }
        bsp_can_send(&control_temp);
        os_delay(1);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_on[2][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_on[3][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
    } else {
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_off[0][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_off[1][i];
        }
        bsp_can_send(&control_temp);
        os_delay(1);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_off[2][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_off[3][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
    }
    bsp_can_clear();
}

void senova_control_light(uint8_t state)
{
    uint8_t i;
    uint8_t cmds_on[2][8] = 
    {
        { 0x10, 0x0C, 0x2F, 0x58, 0xA0, 0x03, 0x40, 0x00 },
        { 0x21, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00 },

    };

    uint8_t cmds_off[2][8] = 
    {
        { 0x10, 0x0C, 0x2F, 0x58, 0xA0, 0x03, 0x00, 0x00 },
        { 0x21, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00 },

    };

    logi("%s: state = %d", __func__, state);
    senova_auth();
    os_delay_ms(200);
//    bsp_can_send(&function_test);
    if(state) {
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_on[0][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_on[1][i];
        }
        bsp_can_send(&control_temp);
        os_delay(1);
    } else {
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_off[0][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_off[1][i];
        }
        bsp_can_send(&control_temp);
        os_delay(1);
    }
    bsp_can_clear();
}
void senova_control_sunroof(uint8_t state)
{

}
void senova_control_trunk(uint8_t state)
{
    uint8_t i;
    uint8_t cmds_on[2][8] = 
    {
        { 0x10, 0x0C, 0x2F, 0x58, 0xA5, 0x03, 0x02, 0x00 },
        { 0x21, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00 },

    };

    uint8_t cmds_off[2][8] = 
    {
        { 0x10, 0x0C, 0x2F, 0x58, 0xA5, 0x03, 0x00, 0x00 },
        { 0x21, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00 },

    };

    logi("%s: state = %d", __func__, state);
    senova_auth();
    os_delay_ms(200);
//    bsp_can_send(&function_test);
    if(state) {
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_on[0][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_on[1][i];
        }
        bsp_can_send(&control_temp);
        os_delay(1);
    } else {
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_off[0][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_off[1][i];
        }
        bsp_can_send(&control_temp);
        os_delay(1);
    }
    bsp_can_clear();
}
void senova_control_findcar(uint8_t state)
{
    uint8_t i;
    uint8_t cmds_on[2][8] = 
    {
        { 0x10, 0x0C, 0x2F, 0x58, 0xA4, 0x03, 0x40, 0x00 },
        { 0x21, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00 },

    };

    uint8_t cmds_off[2][8] = 
    {
        { 0x10, 0x0C, 0x2F, 0x58, 0xA4, 0x03, 0x00, 0x00 },
        { 0x21, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00 },

    };

    logi("%s: state = %d", __func__, state);
    senova_auth();
    os_delay_ms(200);
//    bsp_can_send(&function_test);
    if(state) {
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_on[0][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_on[1][i];
        }
        bsp_can_send(&control_temp);
        os_delay(1);
    } else {
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_off[0][i];
        }
        bsp_can_send(&control_temp);
        os_delay_ms(200);
        for(i = 0; i < 8; i++) {
            control_temp.Data[i] = cmds_off[1][i];
        }
        bsp_can_send(&control_temp);
        os_delay(1);
    }
    bsp_can_clear();
}
